#include "stm32f10x.h"                  // Device header
#include "OLED.h"
#include "MPU6050.h"
#include "Mahony.h"
#include "Calibrate.h"
#include "Store.h"
int i = 0;
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};  // 四元数变量，用于表示传感器姿态
float roll, pitch, yaw;  // 欧拉角变量，用于存储计算结果
float gyroX_rad;
float gyroY_rad;
float gyroZ_rad;
float accX_g;
float accY_g;
float accZ_g;
float gyroXOffset, gyroYOffset, gyroZOffset;  // 陀螺仪零偏

int main(void)
{
    OLED_Init();        // 初始化OLED
    MPU6050_Init();     // 初始化MPU6050
    Store_Init();       // 初始化存储模块
    int16_t AccX, AccY, AccZ;    // 定义加速度计数据变量
    int16_t GyroX, GyroY, GyroZ; // 定义陀螺仪数据变量

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;  // 选择GPIO口
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;  // 设置为上拉输入模式
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  // 设置GPIO速度
    GPIO_Init(GPIOB, &GPIO_InitStructure);  // 初始化GPIO

    
    if(MPU6050_GetID()==209)    // 判断MPU6050是否连接成功
    {
        OLED_ShowString(0, 0, "MPU6050 Error!", OLED_6X8);    // 显示错误信息
        OLED_Update();    // 更新屏幕
        while(1);    // 死循环，停止程序运行
    }    
    else
    {
        OLED_ShowString(0, 0, "MPU6050 OK!", OLED_6X8);    // 显示连接成功信息
        OLED_ShowNum(110, 0, MPU6050_GetID(), 3, OLED_6X8);
        OLED_Update();
        
        // 执行陀螺仪零偏校准
        if (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_0) == 0)  // 按键按下
        {
            CalibrateGyro(&gyroXOffset, &gyroYOffset, &gyroZOffset);  // 调用校准函数
            // 使用联合体在不违反严格别名规则的情况下进行类型转换
            union {
                float f;
                uint16_t i[2];
            } u;
            
            // 存储 gyroXOffset
            u.f = gyroXOffset;
            Store_Data[1] = u.i[0];
            Store_Data[2] = u.i[1];
            
            // 存储 gyroYOffset
            u.f = gyroYOffset;
            Store_Data[3] = u.i[0];
            Store_Data[4] = u.i[1];
            
            // 存储 gyroZOffset
            u.f = gyroZOffset;
            Store_Data[5] = u.i[0];
            Store_Data[6] = u.i[1];
            
            Store_Save();  // 保存数据到闪存
        }
        
        // 从存储中读取陀螺仪零偏
        union {
            float f;
            uint16_t i[2];
        } u;
        
        // 读取 gyroXOffset
        u.i[0] = Store_Data[1];
        u.i[1] = Store_Data[2];
        gyroXOffset = u.f;
        
        // 读取 gyroYOffset
        u.i[0] = Store_Data[3];
        u.i[1] = Store_Data[4];
        gyroYOffset = u.f;
        
        // 读取 gyroZOffset
        u.i[0] = Store_Data[5];
        u.i[1] = Store_Data[6];
        gyroZOffset = u.f;
        
        // 显示陀螺仪零偏值
        OLED_ShowFloatNum(0, 32, gyroXOffset, 3, 3, OLED_6X8);
        OLED_ShowFloatNum(0, 40, gyroYOffset, 3, 3, OLED_6X8);
        OLED_ShowFloatNum(0, 48, gyroZOffset, 3, 3, OLED_6X8);
        
    }

    while(1)
    {
        MPU6050_GetData(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);  // 获取数据
          // 将原始数据转换为物理单位并减去零偏
        // MPU6050配置为±2000°/s (GYRO_CONFIG=0x18)，输出范围为±32768
        // 将原始数据转换为弧度/秒（公式：raw * 2000 / 32768 * π / 180）
        gyroX_rad = GyroX * 0.001065f - gyroXOffset;  // 0.001065 = (2000/32768)*(π/180)
        gyroY_rad = GyroY * 0.001065f - gyroYOffset;
        gyroZ_rad = GyroZ * 0.001065f - gyroZOffset;
          // 加速度计数据转换，配置为±16g (ACCEL_CONFIG=0x18)，输出范围为±32768
        accX_g = AccX / 2048.0f;  // 2048 LSB/g for ±16g range
        accY_g = AccY / 2048.0f;
        accZ_g = AccZ / 2048.0f;
        
        // 使用Mahony算法更新四元数 - 使用默认采样时间
        MahonyAHRSupdate(q, gyroX_rad, gyroY_rad, gyroZ_rad, accX_g, accY_g, accZ_g, 0.0f);
        
        // 计算欧拉角
        QuaternionToEuler(q, &roll, &pitch, &yaw);
        
        // 显示数据
        if(i == 10){
            i=0;
            // 显示欧拉角
            OLED_ShowString(0, 8, "Roll:", OLED_6X8);
            OLED_ShowFloatNum(40, 8, roll, 3, 2, OLED_6X8);
            
            OLED_ShowString(0, 16, "Pitch:", OLED_6X8);
            OLED_ShowFloatNum(40, 16, pitch, 3, 2, OLED_6X8);
            
            OLED_ShowString(0, 24, "Yaw:", OLED_6X8);
            OLED_ShowFloatNum(40, 24, yaw, 3, 2, OLED_6X8);
            // 每次显示后立即更新OLED屏幕
            OLED_Update();
        }

        i++;    // 循环计数器
        
    }
}
